{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "FAZsjB3IN9JN"
   },
   "source": [
    "<center>\n",
    "<h4>CDS 110, Lecture 9</h4>\n",
    "<font color=blue><h1>PID Control of a Servomechanism</h1></font>\n",
    "<h3>Richard M. Murray, Winter 2024</h3>\n",
    "</center>\n",
    "\n",
    "[Open in Google Colab](https://colab.research.google.com/drive/1BP0DFHh94tSxAyQetvOEbBEHKrSoVGQW)\n",
    "\n",
    "In this lecture we will use a variety of methods to design proportional (P), proportional-integral (PI), and proportional-integral-derivative (PID) controllers for a cart pendulum system."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "from math import pi\n",
    "try:\n",
    "  import control as ct\n",
    "  print(\"python-control\", ct.__version__)\n",
    "except ImportError:\n",
    "  !pip install control\n",
    "  import control as ct"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "T0rjwp1mONm1"
   },
   "source": [
    "## System dynamics\n",
    "\n",
    "Consider a simple mechanism consisting of a spring loaded arm that is driven by a  motor, as shown below:\n",
    "\n",
    "<center><img src=\"https://www.cds.caltech.edu/~murray/courses/cds110/sp2024/servomech-diagram.png\" width=200 alt=\"servomech-diagram\"></center>\n",
    "\n",
    "The motor applies a torque that twists the arm against a linear spring and moves the end of the arm across a rotating platter. The input to the system is the motor torque $\\tau_\\text{m}$. The force exerted by the spring is a nonlinear function of the head position due to the way it is attached.\n",
    "\n",
    "The equations of motion for the system are given by\n",
    "\n",
    "$$\n",
    "J \\ddot \\theta = -b \\dot\\theta - k r\\sin\\theta + \\tau_\\text{m},\n",
    "$$\n",
    "\n",
    "which can be written in state space form as\n",
    "\n",
    "$$\n",
    "\\frac{d}{dt} \\begin{bmatrix} \\theta \\\\ \\theta \\end{bmatrix} =\n",
    "  \\begin{bmatrix} \\dot\\theta \\\\ -k r \\sin\\theta / J - b\\dot\\theta / J \\end{bmatrix}\n",
    "  + \\begin{bmatrix} 0 \\\\ 1/J \\end{bmatrix} \\tau_\\text{m}.\n",
    "$$\n",
    "\n",
    "The system parameters are given by\n",
    "\n",
    "$$\n",
    "k = 1,\\quad J = 100,\\quad b = 10,\n",
    "\\quad r = 1,\\quad l = 2,\\quad \\epsilon = 0.01.\n",
    "$$\n",
    "\n",
    "and we assume that time is measured in milliseconds (ms) and distance in centimeters (cm).  (The constants here are made up and don't necessarily reflect a real disk drive, though the units and time constants are motivated by computer disk drives.)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Parameter values\n",
    "servomech_params = {\n",
    "    'J': 100,             # Moment of inertia of the motor\n",
    "    'b': 10,              # Angular damping of the arm\n",
    "    'k': 1,               # Spring constant\n",
    "    'r': 1,               # Location of spring contact on arm\n",
    "    'l': 2,               # Distance to the read head\n",
    "    'eps': 0.01,          # Magnitude of velocity-dependent perturbation\n",
    "}\n",
    "\n",
    "# State derivative\n",
    "def servomech_update(t, x, u, params):\n",
    "    # Extract the configuration and velocity variables from the state vector\n",
    "    theta = x[0]                # Angular position of the disk drive arm\n",
    "    thetadot = x[1]             # Angular velocity of the disk drive arm\n",
    "    tau = u[0]                  # Torque applied at the base of the arm\n",
    "\n",
    "    # Get the parameter values\n",
    "    J, b, k, r = map(params.get, ['J', 'b', 'k', 'r'])\n",
    "\n",
    "    # Compute the angular acceleration\n",
    "    dthetadot = 1/J * (\n",
    "        -b * thetadot - k * r * np.sin(theta) + tau)\n",
    "\n",
    "    # Return the state update law\n",
    "    return np.array([thetadot, dthetadot])\n",
    "\n",
    "# System output (full state)\n",
    "def servomech_output(t, x, u, params):\n",
    "    l = params['l']\n",
    "    return l * x[0]\n",
    "\n",
    "# System dynamics\n",
    "servomech = ct.nlsys(\n",
    "    servomech_update, servomech_output, name='servomech',\n",
    "    params=servomech_params,\n",
    "    states=['theta_', 'thdot_'],\n",
    "    outputs=['y'], inputs=['tau'])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "n4bQu0e2_aBT"
   },
   "source": [
    "In addition to the system dynamics, we assume there are actuator dynamics that limit the performance of the system.  We take these as first order dynamics with saturation:\n",
    "\n",
    "$$\n",
    "\\tau = \\text{sat} \\left(\\frac{\\alpha}{s + \\alpha} u\\right)\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "actuator_params = {\n",
    "    'umax': 5,            # Saturation limits\n",
    "    'alpha': 10,          # Actuator time constant\n",
    "}\n",
    "\n",
    "def actuator_update(t, x, u, params):\n",
    "  # Get parameter values\n",
    "  alpha = params['alpha']\n",
    "  umax = params['umax']\n",
    "\n",
    "  # Clip the input\n",
    "  u_clip = np.clip(u, -umax, umax)\n",
    "\n",
    "  # Actuator dynamics\n",
    "  return -alpha * x + alpha * u_clip\n",
    "\n",
    "actuator = ct.nlsys(\n",
    "    actuator_update, None, params=actuator_params,\n",
    "    inputs='u', outputs='tau', states=1, name='actuator')\n",
    "\n",
    "system = ct.series(actuator, servomech)\n",
    "system.name = 'system'  # missing feature\n",
    "print(system)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "8HYyndF_saE0"
   },
   "source": [
    "### Linearization\n",
    "\n",
    "To study the open loop dynamics of the system, we compute the linearization of the dynamics about the equilibrium point corresponding to $\\theta_\\text{e} = 15^\\circ$."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Convert the equilibrium angle to radians\n",
    "theta_e = (15 / 180) * np.pi\n",
    "\n",
    "# Compute the input required to hold this position\n",
    "u_e = servomech.params['k'] * servomech.params['r'] * np.sin(theta_e)\n",
    "print(\"Equilibrium torque = %g\" % u_e)\n",
    "\n",
    "# Linearize the system dynamics about the equilibrium point\n",
    "P = ct.tf(\n",
    "    system.linearize([0, theta_e, 0], u_e, copy_names=True)[0, 0])\n",
    "P.name = 'P'  # bug\n",
    "print(P, end=\"\\n\\n\")\n",
    "\n",
    "ct.bode_plot(P)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "J1dwXObJSKp-"
   },
   "source": [
    "## Ziegler-Nichols tuning\n",
    "\n",
    "Ziegler-Nichols tuning provides a method for choosing the gains of a PID controller that give reasonable closed loop response.  More information can be found in [Feedback Systems](https://fbswiki.org/wiki/index.php/Feedback_Systems:_An_Introduction_for_Scientists_and_Engineers) (FBS2e), Section 11.3.\n",
    "\n",
    "We show here the figures and tables that we will use (from FBS2e):\n",
    "\n",
    "<center>\n",
    "<table>\n",
    "<tr>\n",
    "<td align='middle'>\n",
    "<img src=\"https://www.cds.caltech.edu/~murray/courses/cds110/sp2024/zn-step-response.png\" width=300>\n",
    "</td>\n",
    "<td align='middle'>\n",
    "<img src=\"https://www.cds.caltech.edu/~murray/courses/cds110/sp2024/zn-step-table.png\" width=300>\n",
    "</td>\n",
    "</tr>\n",
    "</center>\n",
    "\n",
    "To use the Ziegler-Nichols turning rules, we plot the step response, compute the parameters (shown in the figure), and then apply the formulas in the table:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Plot the step response\n",
    "resp = ct.step_response(P)\n",
    "resp.plot()\n",
    "\n",
    "# Find the point of the steepest slope\n",
    "slope = np.diff(resp.outputs) / np.diff(resp.time)\n",
    "mxi = np.argmax(slope)\n",
    "mx_time = resp.time[mxi]\n",
    "mx_out= resp.outputs[mxi]\n",
    "plt.plot(resp.time[mxi], resp.outputs[mxi], 'ro')\n",
    "\n",
    "# Draw a line going through the point of max slope\n",
    "mx_slope = slope[mxi]\n",
    "timepts = np.linspace(0, mx_time*2)\n",
    "plt.plot(timepts, mx_out + mx_slope * (timepts - mx_time), 'r-')\n",
    "\n",
    "# Solve for the Ziegler-Nichols parameters\n",
    "a = -(mx_out - mx_slope * mx_time)  # Find the value of the line at t = 0\n",
    "tau = a / mx_slope                  # Solve a + mx_slope * tau = 0\n",
    "print(f\"{a=}, {tau=}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can then construct a controller using the parameters:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "s = ct.tf('s')\n",
    "\n",
    "# Proportional controller\n",
    "kp = 1/a\n",
    "ctrl_zn_P = kp\n",
    "\n",
    "# PI controller\n",
    "kp = 0.9/a\n",
    "Ti = tau/0.3; ki = kp/Ti\n",
    "ctrl_zn_PI = kp + ki / s\n",
    "\n",
    "# PID controller\n",
    "kp = 1.2/a\n",
    "Ti = tau/0.5; ki = kp/Ti\n",
    "Td = 0.5 * tau; kd = kp * Td\n",
    "ctrl_zn_PID = kp + ki / s + kd * s\n",
    "\n",
    "print(ctrl_zn_PID)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Compute the closed loop systems and plots the step and\n",
    "# frequency responses.\n",
    "\n",
    "clsys_zn_P = ct.feedback(P * ctrl_zn_P)\n",
    "clsys_zn_P.name = 'P'\n",
    "\n",
    "clsys_zn_PI = ct.feedback(P * ctrl_zn_PI)\n",
    "clsys_zn_PI.name = 'PI'\n",
    "\n",
    "clsys_zn_PID = ct.feedback(P * ctrl_zn_PID)\n",
    "clsys_zn_PID.name = 'PID'\n",
    "\n",
    "# Plot the step responses\n",
    "resp.sysname = 'open_loop'\n",
    "resp.plot(color='k')\n",
    "\n",
    "stepresp_zn_P = ct.step_response(clsys_zn_P)\n",
    "stepresp_zn_P.plot(color='b')\n",
    "\n",
    "stepresp_zn_PI = ct.step_response(clsys_zn_PI)\n",
    "stepresp_zn_PI.plot(color='r')\n",
    "\n",
    "stepresp_zn_PID = ct.step_response(clsys_zn_PID)\n",
    "stepresp_zn_PID.plot(color='g')\n",
    "plt.legend()\n",
    "\n",
    "plt.figure()\n",
    "ct.bode_plot([clsys_zn_P, clsys_zn_PI, clsys_zn_PID]);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "6iZwB2WEeg8S"
   },
   "source": [
    "## Loop shaping\n",
    "\n",
    "A better design can be obtained by looking at the loop transfer function and adjusting the controller parameters to give a loop shape that will give closed loop properties.  We show the steps for such a design here:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Design parameters\n",
    "Td = 1                    # Set to gain crossover frequency\n",
    "Ti = Td * 10              # Set to low frequency region\n",
    "kp = 500                  # Tune to get desired bandwith\n",
    "\n",
    "# Updated gains\n",
    "kp = 150\n",
    "Ti = Td * 5; kp = 150\n",
    "\n",
    "# Compute controller parmeters\n",
    "ki = kp/Ti\n",
    "kd = kp * Td\n",
    "\n",
    "# Controller transfer function\n",
    "ctrl_shape = kp + ki / s + kd * s\n",
    "ctrl_shape.name = 'C'\n",
    "\n",
    "# Frequency response (open loop) - use this to help tune your design\n",
    "ltf_shape = P * ctrl_shape\n",
    "ltf_shape.name = 'L'\n",
    "\n",
    "ct.frequency_response([P, ctrl_shape]).plot()\n",
    "ct.frequency_response(ltf_shape).plot(margins=True);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Compute the closed loop systemsand plot the step response\n",
    "# and Nyquist plot (to make sure margins look OK)\n",
    "\n",
    "# Create the closed loop systems\n",
    "clsys_shape = ct.feedback(P * ctrl_shape)\n",
    "clsys_shape.name = 'loopshape'\n",
    "\n",
    "# Step response\n",
    "plt.subplot(2, 1, 1)\n",
    "stepresp_shape = ct.step_response(clsys_shape)\n",
    "stepresp_shape.plot(color='b')\n",
    "plt.plot([0, stepresp_shape.time[-1]], [1, 1], 'k--')\n",
    "\n",
    "# Compare to the ZN controller\n",
    "ax = plt.subplot(2, 1, 2)\n",
    "ct.step_response(clsys_shape, stepresp_zn_PID.time).plot(\n",
    "    color='b', ax=np.array([[ax]]))\n",
    "stepresp_zn_PID.plot(color='g', ax=np.array([[ax]]))\n",
    "ax.plot([0, stepresp_shape.time[-1]], [1, 1], 'k--')\n",
    "\n",
    "# Nyquist plot\n",
    "plt.figure()\n",
    "ct.nyquist([ltf_shape])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We see that the loop shaping controller has better step response (faster rise and settling time, less overshoot)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "GyXQXykafzWs"
   },
   "source": [
    "### Gang of Four\n",
    "\n",
    "When designing a controller, it is important to look at all of the input/output responses, not just the response from reference to output (which is what the step response above focuses on). \n",
    "\n",
    "In the frequency domain, the Gang of 4 plots provide useful information on all (important) input/output pairs:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ct.gangof4(P, ctrl_shape)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "These all look pretty resonable, except that the transfer function from the reference $r$ to the system input $u$ is getting large at high frequency.  This occurs because we did not filter the derivative on the PID controller, so high frequency components of the reference signal (or the measurement noise!) get amplified.  We will fix this in the more advanced controller below."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "uFO3wiWXhBAK"
   },
   "source": [
    "## Anti-windup + derivative filtering\n",
    "\n",
    "In addition to the amplification of high frequency signals due to the derivative term, another practical consideration in the use of PID controllers is integrator windup.  Integrator windup occurs when there are limits on the control inputs so that the error signal may not descrease quickly.  This causes the integral term in the PID controller to see an error for a long period of time, and the resulting integration of the error must be offset by making the error have opposite sign for some period of time.  This is often undesireable.\n",
    "\n",
    "To see how to address both amplification of noise due to the derivative term and integrator windup effects in the presence of input constraints, we now implement PID controller with anti-windup and derivative filtering, as shown in the following figure (see also Figure 11.11 in [FBS2e](https://fbswiki.org/wiki/index.php/Feedback_Systems:_An_Introduction_for_Scientists_and_Engineers)):\n",
    "\n",
    "<center>\n",
    "<img src=\"https://www.cds.caltech.edu/~murray/courses/cds110/sp2024/pid-aw-diagram.png\"</img>\n",
    "</center>\n",
    "\n",
    "### Low pass filter\n",
    "\n",
    "The low pass filtered derivative has transfer function\n",
    "\n",
    "$$\n",
    "G(s) = \\frac{a\\, s}{s + a}.\n",
    "$$\n",
    "\n",
    "This can be implemented using the differential equation\n",
    "\n",
    "$$\n",
    "\\dot \\xi = -a \\xi + a y, \\qquad\n",
    "\\eta = -a \\xi + a y.\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ctrl_params = {'kaw': 5 * ki, 'a': 10/Td}\n",
    "\n",
    "def ctrl_update(t, x, u, params):\n",
    "  # Get the parameter values\n",
    "  kaw = params['kaw']\n",
    "  a = params['a']\n",
    "  umax_ctrl = params.get('umax_ctrl', actuator.params['umax'])\n",
    "\n",
    "  # Extract the signals into more familiar variable names\n",
    "  r, y = u[0], u[1]\n",
    "  z = x[0]        # integral error\n",
    "  xi = x[1]       # filtered derivative\n",
    "\n",
    "  # Compute the controller components\n",
    "  u_prop = kp * (r - y)\n",
    "  u_int = z\n",
    "  ydt_f = -a * xi + a * (-y)\n",
    "  u_der = kd * ydt_f\n",
    "\n",
    "  # Compute the commanded and saturated outputs\n",
    "  u_cmd = u_prop + u_int + u_der\n",
    "  u_sat = np.clip(u_cmd, -umax_ctrl, umax_ctrl)\n",
    "\n",
    "  dz = ki * (r - y) + kaw * (u_sat - u_cmd)\n",
    "  dxi = -a * xi + a * (-y)\n",
    "  return np.array([dz, dxi])\n",
    "\n",
    "def ctrl_output(t, x, u, params):\n",
    "  # Get the parameter values\n",
    "  kaw = params['kaw']\n",
    "  a = params['a']\n",
    "  umax_ctrl = params.get('umax_ctrl', params['umax'])\n",
    "\n",
    "  # Extract the signals into more familiar variable names\n",
    "  r, y = u[0], u[1]\n",
    "  z = x[0]        # integral error\n",
    "  xi = x[1]       # filtered derivative\n",
    "\n",
    "  # Compute the controller components\n",
    "  u_prop = kp * (r - y)\n",
    "  u_int = z\n",
    "  ydt_f = -a * xi + a * (-y)\n",
    "  u_der = kd * ydt_f\n",
    "\n",
    "  # Compute the commanded and saturated outputs\n",
    "  u_cmd = u_prop + u_int + u_der\n",
    "  u_sat = np.clip(u_cmd, -umax_ctrl, umax_ctrl)\n",
    "\n",
    "  return u_cmd\n",
    "\n",
    "ctrl = ct.nlsys(\n",
    "    ctrl_update, ctrl_output, name='ctrl', params=ctrl_params,\n",
    "    inputs=['r', 'y'], outputs=['u'], states=2)\n",
    "\n",
    "clsys = ct.interconnect(\n",
    "    [servomech, actuator, ctrl], name='clsys',\n",
    "    inputs=['r'], outputs=['y', 'tau'])\n",
    "print(clsys)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Plot the step responses for the following cases:\n",
    "#\n",
    "# 'linear': the original linear step response (no actuation limits)\n",
    "# 'clipped': PID controller with input limits, but not anti-windup\n",
    "# 'anti-windup': PID controller with anti-windup compensation\n",
    "\n",
    "# Use more time points to get smoother response curves\n",
    "timepts = np.linspace(0, 2*stepresp_shape.time[-1], 500)\n",
    "\n",
    "# Compute the response for the individual cases\n",
    "stepsize = theta_e / 2\n",
    "resp_ln = ct.input_output_response(\n",
    "    clsys, timepts, stepsize, params={'umax': np.inf, 'kaw': 0, 'a': 1e3})\n",
    "resp_cl = ct.input_output_response(\n",
    "    clsys, timepts, stepsize, params={'umax': 5, 'kaw': 0, 'a': 100})\n",
    "resp_aw = ct.input_output_response(\n",
    "    clsys, timepts, stepsize, params={'umax': 5, 'kaw': 2*ki, 'a': 100})\n",
    "\n",
    "# Plot the time responses in a single plot\n",
    "ct.time_response_plot(resp_ln, color='b', plot_inputs=False, label=\"linear\")\n",
    "ct.time_response_plot(resp_cl, color='r', plot_inputs=False, label=\"clipped\")\n",
    "ct.time_response_plot(resp_aw, color='g', plot_inputs=False, label=\"anti-windup\");"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "DZS7v0EmdK3H"
   },
   "source": [
    "The response of the anti-windup compensator is very sluggish, indicating that we may be setting $k_\\text{aw}$ too high."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "resp_aw = ct.input_output_response(\n",
    "    clsys, timepts, stepsize, params={'umax': 5, 'kaw': 0.05 * ki, 'a': 100})\n",
    "\n",
    "# Plot the time responses in a single plot\n",
    "ct.time_response_plot(resp_ln, color='b', plot_inputs=False, label=\"linear\")\n",
    "ct.time_response_plot(resp_cl, color='r', plot_inputs=False, label=\"clipped\")\n",
    "ct.time_response_plot(resp_aw, color='g', plot_inputs=False, label=\"anti-windup\");"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "pCp_pu0Kh62b"
   },
   "source": [
    "This gives a much better response, though the value of $k_\\text{aw}$ falls well outside the range of [2, 10].  One reason for this is that $k_\\text{aw}$ acts on the inputs, $\\tau$, which are roughly 100 larger than the size of the outputs, $y$, as seen in the above plots."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "1FVGh3k0Y7vB"
   },
   "source": [
    "We can now see if this affects the Gang of Four in the expected way:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "C = ctrl.linearize([0, 0], 0, params=resp_aw.params)[0, 1]\n",
    "ct.gangof4(P, C);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "vT1WfhRHb2ZU"
   },
   "source": [
    "Note that in the transfer function from $r$ to $u$ (which is the same as the transfer function from $e$ to $u$, the high frequency gain is now bounded.  (We could make it go back down by using a second order filter.)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "language_info": {
   "name": "python"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
